
/**
 * From QT source code
 */


#ifndef M3D_MATH3D_H_
#define M3D_MATH3D_H_

#include "mmath.h"
#include "m3d.h"

M3D_BEGIN_NAMESPACE

/***********************************/
/************ MVector3D ************/
/***********************************/

class MVector3D {
public:
    MVector3D();
 	MVector3D(mdouble xpos, mdouble ypos, mdouble zpos);
	MVector3D(mdouble xpos, mdouble ypos, mdouble zpos, int dummy);
    ~MVector3D() {};

	mdouble x() const;
    mdouble y() const;
    mdouble z() const;

    void setX(mdouble x);
    void setY(mdouble y);
    void setZ(mdouble z);

	MVector3D normalized() const;
    void normalize();

    static MVector3D crossProduct(const MVector3D& v1, const MVector3D& v2);
    static mdouble dotProduct(const MVector3D& v1, const MVector3D& v2);

	MVector3D div(const MVector3D &vector, mdouble divisor) const;

private:
   	mdouble xp, yp, zp;
};

inline MVector3D::MVector3D() : xp(0.0f), yp(0.0f), zp(0.0f) {}
inline MVector3D::MVector3D(mdouble xpos, mdouble ypos, mdouble zpos) : xp(xpos), yp(ypos), zp(zpos) {}
inline MVector3D::MVector3D(mdouble xpos, mdouble ypos, mdouble zpos, int) : xp(xpos), yp(ypos), zp(zpos) {}

inline mdouble MVector3D::x() const { return xp; }
inline mdouble MVector3D::y() const { return yp; }
inline mdouble MVector3D::z() const { return zp; }

inline void MVector3D::setX(mdouble aX) { xp = aX; }
inline void MVector3D::setY(mdouble aY) { yp = aY; }
inline void MVector3D::setZ(mdouble aZ) { zp = aZ; }


inline MVector3D MVector3D::div(const MVector3D &vector, mdouble divisor) const
{
   	return MVector3D(vector.x() / divisor, vector.y() / divisor, vector.z() / divisor, 1);
}

/***********************************/
/************ MQuaternion **********/
/***********************************/
class MQuaternion{

public:
	MQuaternion();
	MQuaternion(mdouble scalar, mdouble xpos, mdouble ypos, mdouble zpos);

   	mdouble x() const;
   	mdouble y() const;
   	mdouble z() const;
   	mdouble scalar() const;

    void setX(mdouble x);
    void setY(mdouble y);
    void setZ(mdouble z);
    void setScalar(mdouble scalar);


	MQuaternion normalized() const;

	MQuaternion div(const MQuaternion &quaternion, mdouble divisor) const;

   	static MQuaternion fromAxisAndAngle(const MVector3D& axis, mdouble angle);

	friend inline const MQuaternion operator*(const MQuaternion &q1, const MQuaternion& q2);		

private:
   	mdouble wp, xp, yp, zp;
};

inline MQuaternion::MQuaternion() : wp(1.0f), xp(0.0f), yp(0.0f), zp(0.0f) {}
inline MQuaternion::MQuaternion(mdouble aScalar, mdouble xpos, mdouble ypos, mdouble zpos) : 
		wp(aScalar), xp(xpos), yp(ypos), zp(zpos) {}


inline mdouble MQuaternion::x() const { return xp; }
inline mdouble MQuaternion::y() const { return yp; }
inline mdouble MQuaternion::z() const { return zp; }
inline mdouble MQuaternion::scalar() const { return wp; }

inline void MQuaternion::setX(mdouble aX) { xp = aX; }
inline void MQuaternion::setY(mdouble aY) { yp = aY; }
inline void MQuaternion::setZ(mdouble aZ) { zp = aZ; }
inline void MQuaternion::setScalar(mdouble aScalar) { wp = aScalar; }


inline const MQuaternion operator*(const MQuaternion &q1, const MQuaternion& q2)
{
    mdouble ww = (q1.zp + q1.xp) * (q2.xp + q2.yp);
    mdouble yy = (q1.wp - q1.yp) * (q2.wp + q2.zp);
    mdouble zz = (q1.wp + q1.yp) * (q2.wp - q2.zp);
    mdouble xx = ww + yy + zz;
    mdouble qq = 0.5 * (xx + (q1.zp - q1.xp) * (q2.xp - q2.yp));

    mdouble w = qq - ww + (q1.zp - q1.yp) * (q2.yp - q2.zp);
    mdouble x = qq - xx + (q1.xp + q1.wp) * (q2.xp + q2.wp);
    mdouble y = qq - yy + (q1.wp - q1.xp) * (q2.yp + q2.zp);
    mdouble z = qq - zz + (q1.zp + q1.yp) * (q2.wp - q2.xp);

    return MQuaternion(w, x, y, z);
}


inline MQuaternion MQuaternion::div(const MQuaternion &quaternion, mdouble divisor) const
{
   	return MQuaternion(quaternion.scalar() / divisor, quaternion.x() / divisor, 
				quaternion.y() / divisor, quaternion.z() / divisor);
}

class MMatrix4x4
{
public:

    inline MMatrix4x4() { setToIdentity(); }
    inline MMatrix4x4(mdouble m11, mdouble m12, mdouble m13, mdouble m14,
                      mdouble m21, mdouble m22, mdouble m23, mdouble m24,
                      mdouble m31, mdouble m32, mdouble m33, mdouble m34,
                      mdouble m41, mdouble m42, mdouble m43, mdouble m44);

    //MMatrix4x4(const MMatrix& matrix);

    inline bool isIdentity() const;
    inline void setToIdentity();

	inline const mdouble *data() const { return m[0]; }

    void scale(mdouble x, mdouble y);
    void scale(mdouble x, mdouble y, mdouble z);
    void scale(mdouble factor);
    void translate(mdouble x, mdouble y);
    void translate(mdouble x, mdouble y, mdouble z);
    void rotate(mdouble angle, mdouble x, mdouble y, mdouble z = 0.0f);
    void rotate(const MQuaternion& quaternion);

    inline MMatrix4x4& operator*=(const MMatrix4x4& other);

    friend MMatrix4x4 operator*(const MMatrix4x4& m1, const MMatrix4x4& m2);


private:
	mdouble m[4][4];
    int flagBits;           // Flag bits from the enum below.

    enum {
        Identity        = 0x0001,   // Identity matrix
        General         = 0x0002,   // General matrix, unknown contents
        Translation     = 0x0004,   // Contains a simple translation
        Scale           = 0x0008,   // Contains a simple scale
        Rotation        = 0x0010    // Contains a simple rotation
    };

    MMatrix4x4(int) { flagBits = General; }

};

inline MMatrix4x4::MMatrix4x4
        (mdouble m11, mdouble m12, mdouble m13, mdouble m14,
         mdouble m21, mdouble m22, mdouble m23, mdouble m24,
         mdouble m31, mdouble m32, mdouble m33, mdouble m34,
         mdouble m41, mdouble m42, mdouble m43, mdouble m44)
{
    m[0][0] = m11; m[0][1] = m21; m[0][2] = m31; m[0][3] = m41;
    m[1][0] = m12; m[1][1] = m22; m[1][2] = m32; m[1][3] = m42;
    m[2][0] = m13; m[2][1] = m23; m[2][2] = m33; m[2][3] = m43;
    m[3][0] = m14; m[3][1] = m24; m[3][2] = m34; m[3][3] = m44;
	flagBits = General;
}


inline bool MMatrix4x4::isIdentity() const
{
    if (m[0][0] != 1.0f || m[0][1] != 0.0f || m[0][2] != 0.0f)
        return false;
    if (m[0][3] != 0.0f || m[1][0] != 0.0f || m[1][1] != 1.0f)
        return false;
    if (m[1][2] != 0.0f || m[1][3] != 0.0f || m[2][0] != 0.0f)
        return false;
    if (m[2][1] != 0.0f || m[2][2] != 1.0f || m[2][3] != 0.0f)
        return false;
    if (m[3][0] != 0.0f || m[3][1] != 0.0f || m[3][2] != 0.0f)
        return false;
    return (m[3][3] == 1.0f);
}

inline void MMatrix4x4::setToIdentity()
{
    m[0][0] = 1.0f;
    m[0][1] = 0.0f;
    m[0][2] = 0.0f;
    m[0][3] = 0.0f;
    m[1][0] = 0.0f;
    m[1][1] = 1.0f;
    m[1][2] = 0.0f;
    m[1][3] = 0.0f;
    m[2][0] = 0.0f;
    m[2][1] = 0.0f;
    m[2][2] = 1.0f;
    m[2][3] = 0.0f;
    m[3][0] = 0.0f;
    m[3][1] = 0.0f;
    m[3][2] = 0.0f;
    m[3][3] = 1.0f;

    flagBits = Identity;

}

inline MMatrix4x4& MMatrix4x4::operator*=(const MMatrix4x4& other)
{
    if (flagBits == Identity) {
        *this = other;
        return *this;
    } else if (other.flagBits == Identity) {
        return *this;
    } else {
        *this = *this * other;
        return *this;
    }
}

inline MMatrix4x4 operator*(const MMatrix4x4& m1, const MMatrix4x4& m2)
{
    if (m1.flagBits == MMatrix4x4::Identity)
        return m2;
    else if (m2.flagBits == MMatrix4x4::Identity)
        return m1;

    MMatrix4x4 m(1);
    m.m[0][0] = m1.m[0][0] * m2.m[0][0] +
                m1.m[1][0] * m2.m[0][1] +
                m1.m[2][0] * m2.m[0][2] +
                m1.m[3][0] * m2.m[0][3];
    m.m[0][1] = m1.m[0][1] * m2.m[0][0] +
                m1.m[1][1] * m2.m[0][1] +
                m1.m[2][1] * m2.m[0][2] +
                m1.m[3][1] * m2.m[0][3];
    m.m[0][2] = m1.m[0][2] * m2.m[0][0] +
                m1.m[1][2] * m2.m[0][1] +
                m1.m[2][2] * m2.m[0][2] +
                m1.m[3][2] * m2.m[0][3];
    m.m[0][3] = m1.m[0][3] * m2.m[0][0] +
                m1.m[1][3] * m2.m[0][1] +
                m1.m[2][3] * m2.m[0][2] +
                m1.m[3][3] * m2.m[0][3];
    m.m[1][0] = m1.m[0][0] * m2.m[1][0] +
                m1.m[1][0] * m2.m[1][1] +
                m1.m[2][0] * m2.m[1][2] +
                m1.m[3][0] * m2.m[1][3];
    m.m[1][1] = m1.m[0][1] * m2.m[1][0] +
                m1.m[1][1] * m2.m[1][1] +
                m1.m[2][1] * m2.m[1][2] +
                m1.m[3][1] * m2.m[1][3];
    m.m[1][2] = m1.m[0][2] * m2.m[1][0] +
                m1.m[1][2] * m2.m[1][1] +
                m1.m[2][2] * m2.m[1][2] +
                m1.m[3][2] * m2.m[1][3];
    m.m[1][3] = m1.m[0][3] * m2.m[1][0] +
                m1.m[1][3] * m2.m[1][1] +
                m1.m[2][3] * m2.m[1][2] +
                m1.m[3][3] * m2.m[1][3];
    m.m[2][0] = m1.m[0][0] * m2.m[2][0] +
                m1.m[1][0] * m2.m[2][1] +
                m1.m[2][0] * m2.m[2][2] +
                m1.m[3][0] * m2.m[2][3];
    m.m[2][1] = m1.m[0][1] * m2.m[2][0] +
                m1.m[1][1] * m2.m[2][1] +
                m1.m[2][1] * m2.m[2][2] +
                m1.m[3][1] * m2.m[2][3];
    m.m[2][2] = m1.m[0][2] * m2.m[2][0] +
                m1.m[1][2] * m2.m[2][1] +
                m1.m[2][2] * m2.m[2][2] +
                m1.m[3][2] * m2.m[2][3];
    m.m[2][3] = m1.m[0][3] * m2.m[2][0] +
                m1.m[1][3] * m2.m[2][1] +
                m1.m[2][3] * m2.m[2][2] +
                m1.m[3][3] * m2.m[2][3];
    m.m[3][0] = m1.m[0][0] * m2.m[3][0] +
                m1.m[1][0] * m2.m[3][1] +
                m1.m[2][0] * m2.m[3][2] +
                m1.m[3][0] * m2.m[3][3];
    m.m[3][1] = m1.m[0][1] * m2.m[3][0] +
                m1.m[1][1] * m2.m[3][1] +
                m1.m[2][1] * m2.m[3][2] +
                m1.m[3][1] * m2.m[3][3];
    m.m[3][2] = m1.m[0][2] * m2.m[3][0] +
                m1.m[1][2] * m2.m[3][1] +
                m1.m[2][2] * m2.m[3][2] +
                m1.m[3][2] * m2.m[3][3];
    m.m[3][3] = m1.m[0][3] * m2.m[3][0] +
                m1.m[1][3] * m2.m[3][1] +
                m1.m[2][3] * m2.m[3][2] +
                m1.m[3][3] * m2.m[3][3];
    return m;
}





M3D_END_NAMESPACE

#endif /* M3D_MATH3D_H_ */
